#pragma once
#include <vector>
#include <opencv2/core/core.hpp>
#include <math.h>
#include "Segment.h";
#include "SegmentHelper.h"

using namespace std;
using namespace cv;


const double DERIV_STEP = 1e-5;
const double ACCEPTED_ERROR = 1e-8;
const int MAX_ITER = 100;

class NLSSolver{
protected:
	virtual double getResidual(const Mat &params, int &residualIndex) = 0;//pure virtual function
	virtual double getDerivative(const Mat &params, int &residualIndex, int &paramIndex) = 0;

	Mat GaussNewton(int observedPoints, const Mat &startingParams)
	{
		Mat params = startingParams.clone();
		Mat r(observedPoints, 1, CV_64F); // residual matrix
		Mat Jr(observedPoints, params.rows, CV_64F); // Jacobian of Func()

		double last_totalSquare = 0;
		for(int i=0; i < MAX_ITER; i++) {
			printf("\n Loop i = %d  -------------------------------------------------\n", i);

			double totalSquare = 0;
			for(int residualIndex=0; residualIndex < observedPoints; residualIndex++) {
				double residual = getResidual(params, residualIndex);
				r.at<double>(residualIndex,0) =   residual;
				totalSquare += r.at<double>(residualIndex,0)*r.at<double>(residualIndex,0);

				for(int paramIndex =0; paramIndex < params.rows; paramIndex ++) 
		            		Jr.at<double>(residualIndex,paramIndex) = getDerivative(params, residualIndex, paramIndex);
			}

			//totalSquare /= m;
			printf("i= %d  totalSquare=%f  \t totalSquare - last_totalSquare = %f \n",i, totalSquare, totalSquare - last_totalSquare);
			if(fabs(totalSquare - last_totalSquare) < ACCEPTED_ERROR) break;

			Mat delta = ((Jr.t()*Jr)).inv() * Jr.t()*r;
		
			params -= delta;
			last_totalSquare = totalSquare;
		}
		return params;
	}
public:	
	

};
